/*
 * HandFInder.cpp
 *
 *  Created on: 30.11.2012
 *      Author: root
 */
#include "HandFinder.h"
#include <math.h>
#include <stdlib.h>

HandFinder::HandFinder(uchar _data[WIDTH][HEIGHT][3],int _w,int _h,float _prec,float _prec_a,uchar _Y,uchar _Cb,uchar _Cr,int _color_precision)
{
		for (int i=0;i<_h;i++)
		{
			for (int j=0;j<_w;j++)
			{
				data[j][i][0] = _data[j][i][0];
				data[j][i][1] = _data[j][i][1];
				data[j][i][2] = _data[j][i][2];
			}
		}
        w = _w;
        h = _h;
        prec = _prec;
        prec_a = _prec_a;
        Y = _Y;
        Cb = _Cb;
        Cr = _Cr;
        color_precision = _color_precision;
}

HandFinder::~HandFinder(void)
{
}

void HandFinder::setColor(uchar _Y,uchar _Cb,uchar _Cr)
{
        Y = _Y;
        Cb = _Cb;
        Cr = _Cr;
}
bool HandFinder::ifOutOfArea(int x,int rc,int y,int rs)
{
        return ((abs(getItem(check(x+rc,w),check(y+rs,h),0)-Y)>color_precision)
                || (abs(getItem(check(x+rc,w),check(y+rs,h),1)-Cb)>color_precision)
                || (abs(getItem(check(x+rc,w),check(y+rs,h),2)-Cr)>color_precision));
}
bool HandFinder::ifInArea(int x,int rc,int y,int rs)
{
        return ((abs(getItem(check(x+rc,w),check(y+rs,h),0)-Y)<color_precision)
                && (abs(getItem(check(x+rc,w),check(y+rs,h),1)-Cb)<color_precision)
                && (abs(getItem(check(x+rc,w),check(y+rs,h),2)-Cr)<color_precision));
}

void HandFinder::getPos(int &_x, int &_y)
{
        _x = pos_x;
        _y = pos_y;
}

uchar HandFinder::getItem(int x,int y,int z)
{
        return data[x][y][z];
}

int HandFinder::check(int x,int max_x)
{
        return ((x)>(max_x))?(max_x-1):(((x)<(0))?(0):(x));
}

bool HandFinder::detectHandFromPoint(int x,int y)
{
        if ( (x<w) && (x>=0) && (y<h) && (y>=0) )
        {
                if (ifOutOfArea(x,0,y,0))
                {
					findOut(x,y);
                }
                findCenterGeometric(this->pos_x,this->pos_y);
                return true;
        }
        else
        {
                return false;
        }
}
float HandFinder::radius_scale(int r,int a)
{
	return r*(1-cos((float)a/2));
}
void HandFinder::findCenterGeometric(int x,int y)
{
    float res_x=prec,res_y=prec,d=0,max=0,min=0;
	int max_i=-1;
	int n = (int)(6.28/prec_a);
	float *di = new float[n];
	float *vx = new float[n];
	float *vy = new float[n];
	for (int i=0;i<n;i++)
	{
		vx[i] = cos((i*6.28)/n);
		vy[i] = sin((i*6.28)/n);
	}
	bool end = false;
	if (ifInArea(x,0,y,0))
	{
		while (!((res_x<prec) && (res_x>-prec) && (res_y<prec) && (res_y>-prec)))
		{
			d = 0;
			max = 0;
			min = w;
			max_i = -1;
			end = false;
			while (!end)
			{
				end = true;
				for (int i=0;i<n;i++)
				{
					if (d==0)
					{
						di[i]=0;
						end = false;
					}
					else 
					{
						if (di[i] == 0)
						{
							end = false;
							if (ifOutOfArea(x,d*vx[i],y,d*vy[i]))
							{
								di[i] = d;
								if (d < min)
								{
									min = d;
								}
								if (d > max)
								{
									max = d;
									max_i = i;
								}
							}
						}
					}
				}
				d+=prec;
				if ((d>w) && (d>h))
				{
					delete di;
					delete vx;
					delete vy;
					return;
				}
			}
			res_x = 0;
			res_y = 0;
			if (max > min*10)
			{
				x += res_x = vx[max_i]*di[max_i]/2;
				y += res_y = vy[max_i]*di[max_i]/2;
			}
			else
			{
				for (int i=0;i<n;i++)
				{
					res_x += vx[i]*di[i];
					res_y += vy[i]*di[i];
				}
				x += res_x = res_x/n;
				y += res_y = res_y/n;
			}
		}
	}
	else
	{
		delete di;
		delete vx;
		delete vy;
		return;
	}
    pos_x = x;
    pos_y = y;
	delete di;
	delete vx;
	delete vy;
	return;
}
void HandFinder::findIn(int x,int y)
{
        float res_x=0,res_y=0;
        float da=0;
        bool inside = true;
        bool revers = false;
		bool correct_zone = false;
        for (float r=prec*2;(r<(w-x)) && (r<(h-y)) && (r<x) && (r<y);r+=prec)
        {
                for (float a=0;a<6.28;a+=prec_a)
                {
                        int rc=r*cos(a),rs=r*sin(a);
                        if (inside && ifOutOfArea(x,rc,y,rs))
                        {
                                inside = false;
                                da = a;
                        }
                        if (!inside && ( !(a+prec_a<6.28) || ifInArea(x,rc,y,rs) ))
                        {
                                inside = true;
                                //revers = true;
                                res_x += /*abs(da-a)**/-radius_scale(r,a+da)*cos((a+da)/2);
                                res_y += /*abs(da-a)**/-radius_scale(r,a+da)*sin((a+da)/2);
                        }
                }
                if ((res_x<prec) && (res_x>-prec) && (res_y<prec) && (res_y>-prec))
                {
					if (correct_zone)
					{
						continue;
					}
                }
				else
				{
					correct_zone = true;
				}
                if (revers)
                {
					r-=prec;
                }
                x += res_x/5;
                y += res_y/5;
                res_x = 0;
                res_y = 0;
        }
        pos_x = x;
        pos_y = y;
}
void HandFinder::findOut(int x,int y)
{
        float res_x=0,res_y=0;
        float da=0;
        for (float r=0;(w>h)?(r<w):(r<h);r+=prec)
        {
                bool inside = false;
                for (float a=0;a<6.28;a+=prec_a)
                {
                        int rc=r*cos(a),rs=r*sin(a);
                        if ( !inside && ifInArea(x,rc,y,rs) )
                        {
                                inside = true;
                                da = a;
                                res_x += rc;
                                res_y += rs;
                        }
                        if (inside && ( !(a+prec_a<6.28) && (ifOutOfArea(x,rc,y,rs)) ))
                        {
                                inside = false;
                                //res_x += (a-da)*r*cos(a-da);
                                //res_y += (a-da)*r*sin(a-da);
                        }
                }
                if (!((res_x<1) && (res_x>-1) && (res_y<1) && (res_y>-1)))
                {
                        x += res_x;
                        y += res_y;
                        res_x = 0;
                        res_y = 0;
                        break;
                }
        }
        pos_x = check(x,w);
        pos_y = check(y,h);
}

int HandFinder::analyseRadiusMinMax()
{
        radius_max = -1;
        radius_min = (w>h)?(w):(h);
        for (float a=0;a<6.28;a+=prec_a)
        {
                float r;
                for (r=0;(r<(w-pos_x)) && (r<(h-pos_y)) && (r<pos_x) && (r<pos_y);r+=prec)
                {
                        int rc=r*cos(a),rs=r*sin(a);
                        if (ifOutOfArea(pos_x,rc,pos_y,rs))
                        {
                                break;
                        }
                }
                if (r > radius_max)
                {
                        radius_max = r - prec;
                }
                if (r < radius_min)
                {
                        radius_min = r;
                }
        }
        if ((radius_max != -1) && (radius_min != (w>h)?(w):(h)))
        {
                return (int)(radius_max - radius_min);
        }
        else
        {
                return 0;
        }
}

bool HandFinder::detectFingers()
{
        finger_down_x.clear();
        finger_down_y.clear();
        finger_down_a.clear();
        finger_up_x.clear();
        finger_up_y.clear();
        finger_up_a.clear();
        float da=0;
        bool switcher = false;
        float rad = (radius_max - radius_min);
        for (float dr=rad/3;dr<rad;dr+=rad/3)
        {
                float a=0;
                int rc=dr*cos(a),rs=dr*sin(a);
                vector <float> *cur_finger_lvl_x;
                vector <float> *cur_finger_lvl_y;
                vector <float> *cur_finger_lvl_a;
                if (dr<rad/2)
                {
                        cur_finger_lvl_x = &finger_down_x;
                        cur_finger_lvl_y = &finger_down_y;
                        cur_finger_lvl_a = &finger_down_a;
                }
                else
                {
                        cur_finger_lvl_x = &finger_up_x;
                        cur_finger_lvl_y = &finger_up_y;
                        cur_finger_lvl_a = &finger_up_a;
                }
                if ( ifInArea(pos_x,rc,pos_y,rs) )
                {
                        switcher = true;
                        cur_finger_lvl_x->push_back(1);
                        cur_finger_lvl_y->push_back(1);
                        cur_finger_lvl_a->push_back(1);
                }
                else
                {
                        switcher = false;
                        cur_finger_lvl_x->push_back(0);
                        cur_finger_lvl_y->push_back(0);
                        cur_finger_lvl_a->push_back(0);
                }
                for (a=prec_a;a<6.28;a+=prec_a)
                {
                        rc=dr*cos(a);
                        rs=dr*sin(a);
                        if ( ( (switcher) && ifOutOfArea(pos_x,rc,pos_y,rs) )
                                || ( (!switcher) && ifInArea(pos_x,rc,pos_y,rs) ) )
                        {
                                if (switcher)
                                {
                                        cur_finger_lvl_x->push_back(pos_x+rc);
                                        cur_finger_lvl_y->push_back(pos_y+rs);
                                        cur_finger_lvl_a->push_back((a-da)/2);
                                }
                                switcher = !switcher;
                        }
                }
                rc=dr*cos(a);
                rs=dr*sin(a);
                cur_finger_lvl_x->push_back(pos_x+rc);
                cur_finger_lvl_y->push_back(pos_y+rs);
                cur_finger_lvl_a->push_back((a-da)/2);
        }
        return true;
}
int HandFinder::detectMove()
{
        return 0;
}